#include "MPC_Base.h"
#include "../../../ThirdPartyCode/NR_Vector_CVersion.h"
#include "../../../ThirdPartyCode/Utilities.h"
// =============================================
//
// Class: MPC_settings
//
// =============================================
MPC_settings::MPC_settings(int iHorizon)
{
	int i;
	Horizon = iHorizon;
	Weighting = new double[Horizon];
	for(i=0;i<Horizon;i++)
	{
		Weighting[i] = 1.0;
	}
	
	// Default parameters;
	Dens_set   = 0.98;
	Fact_D     = 1.0;
	Rough2_set = 60;
	Fact_R2    = 1.0;
	Ht_set     = 400;
	Fact_H     = 1.0;
	VarR2_set  = 0;
	Fact_VarR2 = 1.0; 
	ControllerID = 1;	
}
// =============================================
MPC_settings::~MPC_settings()
{
	delete [] Weighting;
}
// =============================================
/*int MPC_settings::set(char* VarName,double VarValue)
{
	if strcmp(VarName,"REPEAT") == 0
		Repeat = VarValue;
	else if strcmp(VarName,"Dens_set")==0
		Dens_set = VarValue;
	else if(strcmp(VarName,"Fact_D")==0)
		Fact_D = VarValue;
	else if(strcmp(VarName,"Rough2_set")==0)
		Rough2_set = VarValue;
	else if(strcmp(VarName,"Fact_R2")==0)
		Fact_R2 = VarValue;
	else if(strcmp(VarName,"varR2_set")==0)
		varR2_set = VarValue;
	else if(strcmp(VarName,"Fact_varR2")==0)
		Fact_varR2 = VarValue;
	else if(strcmp(VarName,"Ht_set")==0)
		Ht_set = VarValue;
	else if(strcmp(VarName,"Fact_h")==0)
		Fact_h = VarValue;
	else if(strcmp(VarName,"RT_dep")==0)
		RT_dep = VarValue;
	else if(strcmp(VarName,"DepRateDefault")==0)
		DepRateDefault = VarValue;

	return 1;
}*/
// =============================================


// =============================================
//
// Class: MPC_Base
//
// =============================================
MPC_Base::MPC_Base(	double	iDens_set,
					double	iFact_D, 
					double	iRough2_set,
					double	iFact_R2, 
					double	iHt_set,
					double	iFact_H, 
					double	it_end,
					double  iDepRate_ini,
					double	iRT_Dep,
					double	it_int_ctrl, 
					int		iStep_num,
					int		iRel_err,
					char*   iFileName,
					int		iDetail,
					int		iControllerTypeID)
{
	int i;

	if(iDens_set>1.0 || iDens_set < 0.0)		{cerr << "Error@MPC_Base::MPC_Base(): illegal value of set point of density\n";system("pause");exit(0);}
	if(iFact_D>1.0 || iFact_D < 0.0)			{cerr << "Error@MPC_Base::MPC_Base(): illegal value of weight of density\n";system("pause");exit(0);}
	if(iRough2_set < 0.0)						{cerr << "Error@MPC_Base::MPC_Base(): illegal value of set point of roughness\n";system("pause");exit(0);}
	if(iFact_R2>1.0 || iFact_R2<0.0)			{cerr << "Error@MPC_Base::MPC_Base(): illegal value of weight of roughness\n";system("pause");exit(0);}
	if(iHt_set < 0.0)							{cerr << "Error@MPC_Base::MPC_Base(): illegal value of set point of height\n";system("pause");exit(0);}
	if(iFact_H>1.0 || iFact_H < 0.0)			{cerr << "Error@MPC_Base::MPC_Base(): illegal value of weight of height. iFact_H = " << iFact_H << "\n";system("pause");exit(0);}
	if(iDepRate_ini>0.5|| iDepRate_ini <0.0)	{cerr << "Error@MPC_Base::MPC_Base(): illegal value of DepRate. DepRate = " << iDepRate_ini << "\n";system("pause");exit(0);}
	if(it_int_ctrl <= 0.0)						{cerr << "Error@MPC_Base::MPC_Base(): illegal value of t_int_ctrl. t_int_ctrl = " << it_int_ctrl << "\n";system("pause");exit(0);}
	if(iStep_num <= 0)							{cerr << "Error@MPC_Base::MPC_Base(): illegal value of Step_num. Step_num = " << iStep_num << "\n";system("pause");exit(0);}
	if(iRel_err!=0 && iRel_err!=1)				{cerr << "Error@MPC_Base::MPC_Base(): Rel_err must be 1 or 0. Rel_err = "<< Rel_err << "\n";system("pause");exit(0);}

	t_int_ctrl	= it_int_ctrl;	
	t_begin		= 0.0;
	t_end		= it_end;
	Step_num	= iStep_num;			
	RT_Dep		= iRT_Dep;				
	Dens_set	= iDens_set;		
	Fact_D		= iFact_D;
	Rough2_set	= iRough2_set;
	Fact_R2		= iFact_R2;
	Ht_set		= iHt_set;			
	Fact_H		= iFact_H;
	Rel_err		= iRel_err;
	DepRate_ini	= iDepRate_ini;
	FileName    = iFileName;
	Detail		= iDetail;
	ControllerTypeID = iControllerTypeID;

	Repeated	= 0;
	t_idx		= 0;
	t_node		= int((t_end-t_begin)/t_int_ctrl)+1;

	// Recording variables
	t_arr	= new double[t_node];
	W_sum	= new double[t_node]; 
	W2_sum	= new double[t_node];
	T_sum   = new double[t_node];
	T2_sum  = new double[t_node];
	//T_detail = new signal("T",t_end,t_int_ctrl,1);

	v_dep_rate  = vector(1,Step_num);
	v_T         = vector(1,Step_num);
	uplim		= vector(1,Step_num);
	lolim		= vector(1,Step_num);

	for(i = 0;i<t_node;i++)
	{
		t_arr[i]	= i*t_int_ctrl;
		W_sum[i]	= 0.0;
		W2_sum[i]	= 0.0;
	}
	
	ht			= 0.0;
	R2			= 0.0;
	g_dens		= 0.0;	
	dep_rate	= DepRate_ini;	
	time		= 0.0;
	
	Model = new model(MODE,t_int_ctrl);

	MPC_diag.cost		= 0;
	MPC_diag.Dopt		= new double[Step_num];
	MPC_diag.Hopt		= new double[Step_num];
	MPC_diag.R2opt		= new double[Step_num];
	MPC_diag.varR2opt	= new double[Step_num];
	MPC_diag.exitflag   = 0;
	MPC_diag.ub         = new double[Step_num];
	MPC_diag.lb         = new double[Step_num];

	if(Detail==1) 
	{
		strcpy(FileName2,"");
		strncat(FileName2,FileName,strlen(FileName)-4);
		strcat(FileName2,"_detail.dat");
		cout << "\nMPC1: opening " << FileName2 << " for output.\n";

		fout_detail.open(FileName2);
		fout_detail << "class: MPC_Base v" << VERSION << "\n";
		fout_detail << "Created:\n";
		fout_detail << "Dens_set    = " << Dens_set << "\n";
		fout_detail << "Fact_D      = " << Fact_D << "\n";
		fout_detail << "Rough2_set  = " << Rough2_set << "\n";
		fout_detail << "Fact_R2     = " << Fact_R2 << "\n";
		fout_detail << "Ht_set      = " << Ht_set << "\n";
		fout_detail << "Fact_H      = " << Fact_H << "\n";
		fout_detail << "t_int_ctrl  = " << t_int_ctrl << "\n";
		fout_detail << "Step_num    = " << Step_num << "\n";
		fout_detail << "RT_Dep      = " << RT_Dep << "\n";
		fout_detail << "DepRate_ini = " << DepRate_ini << "\n";
		fout_detail << "Rel_err     = " << Rel_err << "\n";
		
		fout_detail << setw(6) << "Run #" << setw(15) << "t(s)";
		for(i=1;i<=Step_num;i++)	
			fout_detail << setw(20) << "W[i](layer/s)";
		fout_detail << setw(15) << "cost";
		fout_detail << setw(15) << "ht" << setw(15) << "R2" << setw(15) << "g_dens";
		
		for(i=0;i<10;i++)
		{
			fout_detail << setw(18) << "alphas[" << i << "]";
		}
		for(i=10;i<MODE;i++)
		{ 
			fout_detail << setw(17) << "alphas[" << i << "]";
		}
		for(i=0;i<10;i++)
		{
			fout_detail << setw(18) << "betas[" << i << "]";
		}
		for(i=10;i<MODE;i++)
		{
			fout_detail << setw(17) << "betas[" << i << "]";
		}
		for(i=0;i<Step_num;i++) // uplim
		{
			fout_detail << setw(18) << "uplim[" << i << "]";
		}
		for(i=0;i<Step_num;i++) // uplim
		{
			fout_detail << setw(18) << "lolim[" << i << "]";
		}

		fout_diag.open("MPC_Base_diag.dat");
		for(i=0;i<Step_num;i++)
		{
			fout_diag << setw(16) << "Hopt[" << i << "]";
		}
		for(i=0;i<Step_num;i++)
		{
			fout_diag << setw(16) << "Dopt[" << i << "]";
		}
		for(i=0;i<Step_num;i++)
		{
			fout_diag << setw(16) << "R2opt[" << i << "]";
		}
		for(i=0;i<Step_num;i++)
		{
			fout_diag << setw(16) << "varR2opt[" << i << "]";
		}
	}

	db_log.open("db_log_g_dens_SOR.dat");

}
// =============================================
MPC_Base::~MPC_Base()
{
	int i;
	double W_mean;
	double W_var;
	double W_var2;

	ofstream fout;
	fout.open(FileName);
	fout << "class: MPC_Base" << "\n";
	fout << "Dens_set    = " << Dens_set << "\n";
	fout << "Fact_D      = " << Fact_D << "\n";
	fout << "Rough2_set  = " << Rough2_set << "\n";
	fout << "Fact_R2     = " << Fact_R2 << "\n";
	fout << "Ht_set      = " << Ht_set << "\n";
	fout << "Fact_Ht     = " << Fact_H << "\n";
	fout << "t_int_ctrl  = " << t_int_ctrl << "\n";
	fout << "Step_num    = " << Step_num << "\n";
	fout << "RT_Dep      = " << RT_Dep << "\n";
	fout << "DepRate_ini = " << DepRate_ini << "\n";
	fout << "Rel_err     = " << Rel_err << "\n";
	fout << "Repeated    = " << Repeated << "\n";
	fout << setw(6) << "Time(s)" << setw(15) << "<W>(layer/s)" << setw(18) << "Var(W)(layer/s)" << "\n";
	for(i = 0;i<t_node;i++)
	{
		W_mean = W_sum[i]/Repeated;
		
		W_var2 = (W2_sum[i]-W_mean*W_mean*Repeated)/Repeated;
		if(W_var2 < -TOLERANCE)
		{
			cerr << "ERROR@MPC_Base::~MPC_Base().\n";
			cerr << " i=" << i << "Sum(W2)=" << W2_sum[i] << "<W>=" << W_mean << "Var(W)^2=" << W_var2 << "\n";
		}
		else if (W_var2 > -TOLERANCE && W_var2 < 0)
		{
			W_var = 0.0;
		}
		else
			W_var = sqrt(W_var2);
		
		fout << setw(6) << setprecision(10) << t_arr[i];
		fout << setw(15) << setprecision(10) << W_mean;
		fout << setw(18) << setprecision(10) << W_var;
		if(i<t_node-1)  fout << "\n";
	}
	fout.close();
	if(Detail==1) 
	{
		fout_detail.close();
		fout_diag.close();
	}

	delete [] W_sum;
	delete [] W2_sum;
	delete [] T_sum;
	delete [] T2_sum;
	delete [] MPC_diag.Hopt;
	delete [] MPC_diag.Dopt;
	delete [] MPC_diag.R2opt;
	delete [] MPC_diag.varR2opt;
	delete [] MPC_diag.ub;
	delete [] MPC_diag.lb;

	free_vector(lolim,1,Step_num);
	free_vector(uplim,1,Step_num);
	free_vector(v_dep_rate,1,Step_num);
	free_vector(v_T,1,Step_num);

	delete Model;
}
// =============================================
void MPC_Base::reset()
{
	ht		= 0.0;
	//rough	= 0.0;
	R2		= 0.0;
	g_dens	= 0.0;	
	dep_rate	= DepRate_ini;	
	time		= 0.0;
	t_idx		= 0;
	
	W_sum[t_idx] += dep_rate;
	W2_sum[t_idx] +=  dep_rate*dep_rate;

	t_idx ++;
	Repeated += 1;
}
// =============================================
int MPC_Base::notfinish()
{
	return t_idx < t_node;
}
// =============================================
void MPC_Base::log_detail()
{
	int i;
	
	fout_detail << "\n";
	fout_detail << setw(6)  << setprecision(10) << Repeated;
	fout_detail << setw(15) << setprecision(10) << time;
	for(i=1;i<=Step_num;i++)
	{
		fout_detail << setw(20) << setprecision(10) << v_dep_rate[i];
	}
	fout_detail << setw(15) << setprecision(10) << MPC_diag.cost;
	fout_detail << setw(15) << setprecision(10) << ht;
	fout_detail << setw(15) << setprecision(10) << R2;
	fout_detail << setw(15) << setprecision(10) << g_dens;

	for(i=0;i<MODE;i++)	// alphas
	{
		fout_detail << setw(20) << setprecision(10) << alphas[i];
	}
	for(i=0;i<MODE;i++) // betas
	{
		fout_detail << setw(20) << setprecision(10) << betas[i];
	}
	for(i=0;i<Step_num;i++) // uplim
	{
		fout_detail << setw(20) << setprecision(10) << uplim[i+1];
	}
	for(i=0;i<Step_num;i++) // lolim
	{
		fout_detail << setw(20) << setprecision(10) << lolim[i+1];
	}

	db_log << setw(18) << db_g_dens_SOR << setw(18) <<"\n";
	
	fout_diag << "\n";
	for(i=0;i<Step_num;i++)
	{
		fout_diag << setw(18) << MPC_diag.Hopt[i] << " ";
	}
	for(i=0;i<Step_num;i++)
	{
		fout_diag << setw(18) << MPC_diag.Dopt[i] << " ";
	}
	for(i=0;i<Step_num;i++)
	{
		fout_diag << setw(18) << MPC_diag.R2opt[i] << " ";
	}
	for(i=0;i<Step_num;i++)
	{
		fout_diag << setw(18) << MPC_diag.varR2opt[i] << " ";
	}
}
// =============================================
/*double MPC_Base::getT()
{
	return T;
}*/
// =============================================
/*double MPC_Base::getDepRate()
{
	return dep_rate;
}*/
// =============================================
/*double MPC_Base::getTime()
{
	return time;
}*/
// =============================================



// =============================================
//
// UPPER AND LOWER BOUND
//
// =============================================
double MPC_Base::cost_funct(double DEP_RATE[], double para[]) 
// cost_funct will calculate the deviation of SOR and roughness from the set-point values
// temp is the temperature of the substrate
{
	int		i,j;
	double	n;
	int		step_num;
	double	lambda,phi2_ss;	// tau is time constant, phi2_ss is the steady-state value of alpha^2
	double	dens_curr,rough2_curr,ht_curr,varR2_pred;		// values before predictions
	double	dens_pred,rough2_pred,ht_pred;		// sum of predictions at prediction horizons
	double	var0=0.0,cur_time;
	double	dev_total=0,dev_r2,dev_d,dev_h,dev_varR2;
	double	alpha2_pred[MODE],beta2_pred[MODE];		// for cost_funct use only
	double	alpha2_curr[MODE],beta2_curr[MODE];		// for cost_funct use only

	step_num=(int)para[1];						

	// assign current states to the predicting variables
	for(j=0;j<MODE;j++)
	{
		alpha2_curr[j]	= alphas[j]*alphas[j];
		beta2_curr[j]	= betas[j]*betas[j];
	}
	ht_curr=ht;
	dens_curr=g_dens;

	// dev_total = \sum_{i=1}{p}{Fact_D*\frac{
	dev_total=0.0;
	for(i=1;i<=step_num;i++)
	{
		Model->update(DEP_RATE[i]);
		
		// Thickness ---------------------------------
		ht_pred  = Model->getH();
		// if the predicted thickness is less than the set-point, add the deviation square to the cost
		// otherwise do nothing, i.e., penalizing on the under-growth film
		if(ht_pred<Ht_set)	
		{
			dev_h=ht_pred-Ht_set;
		}
		else
		{
			dev_h=0;
		}
		if (Rel_err)
		{
			dev_h/=Ht_set;	// relative error
		}
		dev_h*=dev_h;
		// Porosity ---------------------------------
		dens_pred   = Model->getRho();
		dev_d=dens_pred-Dens_set;
		if (Rel_err)
		{
			dev_d/=Dens_set;	// relative error
		}
		dev_d*=dev_d;		
		// mean value of roughness square -----------
		rough2_pred  = Model->getR2();
		dev_r2=rough2_pred-Rough2_set;
		if (Rel_err)
		{
			dev_r2/=Rough2_set;	// relative error
		}
		dev_r2*=dev_r2;
		// variance of R2 ---------------------------
		varR2_pred = Model->getVarR2();
		dev_varR2  = varR2_pred*varR2_pred;

		dev_total+=Fact_H*dev_h+Fact_D*dev_d+Fact_R2*dev_r2+Fact_varR2*dev_varR2;
	}
	return dev_total;
}
// =============================================
void MPC_Base::upperlimit()
// upper bound
{
	int i;
	double dep_upper;

	for(i=1;i<=Step_num;i++)
	{
		dep_upper=dep_rate+i*t_int_ctrl*RT_Dep;
		uplim[i]=DEP_HIGH>dep_upper?dep_upper:DEP_HIGH;
	}
}
// =============================================
void MPC_Base::lowerlimit()
// lower bound without height constraints
{
	int i;
	double dep_lower;
	for(i=1;i<=Step_num;i++)
	{
		dep_lower=dep_rate-i*t_int_ctrl*RT_Dep;
		lolim[i] = DEP_LOW>dep_lower?DEP_LOW:dep_lower;
	}
}
// =============================================


/*************************************************************************
//
// The default optimizor come with MPC_Base
//
/*************************************************************************
Functions/Optimization problem solver
	int amoeba_constr(double **p, double y[], int ndim, double ftol,
		double (*funk)(double [], double []), double para[], int *nfunk,
		double uplim[], double lolim[]);
	double amotry(double **p, double y[], double psum[], int ndim,
		double (*funk)(double [], double []), double para[], int ihi, double fac);
	double fminsearch(double p0[], int ndim, double ftol,
				double (*funk)(double [], double []), 
				double para[], int *nfunk, double uplim[], double lolim[]);
*************************************************************************/
//int MPC_Base::amoeba_constr(double **p, double y[], int ndim, double ftol,
//		double (*funk)(double [], double []), double para[], int *nfunk,
//		double uplim[], double lolim[])
int MPC_Base::amoeba_constr(double **p, double y[], int ndim, double ftol,
		double para[], int *nfunk,
		double uplim[], double lolim[])
/*********************************************************************************************\
Multidimensional minimization of the function funk(x) where x[1..ndim] is a vector in ndim
dimensions, by the downhill simplex method of Nelder and Mead. The matrix p[1..ndim+1]
[1..ndim] is input. Its ndim+1 rows are ndim-dimensional vectors which are the vertices of
the starting simplex. Also input is the vector y[1..ndim+1], whose components must be pre-
initialized to the values of funk evaluated at the ndim+1 vertices (rows) of p; and ftol the
fractional convergence tolerance to be achieved in the function value (n.b.!). On output, p and
y will have been reset to ndim+1 new points all within ftol of a minimum function value, and
nfunk gives the number of function evaluations taken.
\*********************************************************************************************/
{
//	double amotry(double **p, double y[], double psum[], int ndim,
//			double (*funk)(double [], double []), double para[], int ihi, double fac);
//	double amotry(double **p, double y[], double psum[], int ndim,
//			double para[], int ihi, double fac);
	int i,ihi,ilo,inhi,j,mpts=ndim+1;
	double rtol,sum,swap,ysave,ytry,*psum;
	int outoflimit;

	ilo=0;

	psum=vector(1,ndim);
	*nfunk=0;
	GET_PSUM
	for (;;) {
		ilo=1;
		/* First we must determine which point is the highest (worst), next-highest, and lowest
		(best), by looping over the points in the simplex. */
		ihi = y[1]>y[2] ? (inhi=2,1) : (inhi=1,2);
		for (i=1;i<=mpts;i++) {
			if (y[i] <= y[ilo]) ilo=i;
			if (y[i] > y[ihi]) {
				inhi=ihi;
				ihi=i;
			} 
			else if (y[i] > y[inhi] && i != ihi) inhi=i;
		}
		rtol=2.0*fabs(y[ihi]-y[ilo])/(fabs(y[ihi])+fabs(y[ilo])+TINY);
		/* Compute the fractional range from highest to lowest and return if satisfactory. */
		if (rtol < ftol) { /* If returning, put best point and value in slot 1. */
			SWAP(y[1],y[ilo])
			for (i=1;i<=ndim;i++) SWAP(p[1][i],p[ilo][i])
			break;	// EXIT POINT #1
		}
		// 2009.02.18: if iteration number exceed NMAX, quit loop
		if (*nfunk >= NMAX)
		{
			//nrerror("NMAX exceeded");
			break;	// EXIT POINT #2
		}
		/* If any entry of the p[ilo][i] is larger than the upper limit, stop calculation */
		/* If any entry of the p[ilo][i] is smaller than the lower limit, stop calculation */
		// 2008.08.20 by Hu: Add relationship test (p[ilo][i-1]-t_int_ctrl*RT_Dep<p[ilo][i]<p[ilo][i-1]+t_int_ctrl*RT_Dep)
		outoflimit=0;
		for (i=1;i<=ndim;i++) {
			if(i>1)
			{
				if(p[ilo][i]>p[ilo][i-1]+t_int_ctrl*RT_Dep) {
					outoflimit=1; 
					//p[ilo][i]=p[ilo][i-1]+t_int_ctrl*RT_Dep;	
					break;
				}
				else if(p[ilo][i]<p[ilo][i-1]-t_int_ctrl*RT_Dep){
					outoflimit=1; 
					//p[ilo][i]=p[ilo][i-1]-t_int_ctrl*RT_Dep;	
					break;
				}
			}
			if(p[ilo][i]>uplim[i]) {
				outoflimit=1; 
				//p[ilo][i]=uplim[i];	
				break;
			}
			else if(p[ilo][i]<lolim[i]) {
				outoflimit=1; 
				//p[ilo][i]=lolim[i];	
				break;
			}
		}
		if (outoflimit==1) {break;} // if outoflimit(either large of small), then stop finding

		///* If any entry of the p[ilo][i] is larger than the upper limit, stop calculation */
		//	  outoflimit=0;
		//	  for (i=1;i<=ndim;i++) if (p[ilo][i]>uplim[i]) {
		//		  outoflimit=1; 
		//		  break;
		//	  }
		//	  //if (outoflimit==1) {ilo=mpts+1; break;}
		///* If any entry of the p[ilo][i] is smaller than the lower limit, stop calculation */
		//	  for (i=1;i<=ndim;i++) if (p[ilo][i]<lolim[i]) {outoflimit=1; break;}
		//	  //if (outoflimit==1) {ilo=-1; break;}
		//	  if (outoflimit==1) {break;} // if outoflimit(either large of small), then stop finding

		*nfunk += 2;
		/* Begin a new iteration. First extrapolate by a factor . 1 through the face of the simplex
		across from the high point, i.e., re ect the simplex from the high point. */
		//ytry=amotry(p,y,psum,ndim,funk,para,ihi,-1.0);
		ytry=amotry(p,y,psum,ndim,para,ihi,-1.0);
		if (ytry <= y[ilo])
			/* Gives a result better than the best point, so try an additional extrapolation by a factor 2. */
			//ytry=amotry(p,y,psum,ndim,funk,para,ihi,2.0);
			ytry=amotry(p,y,psum,ndim,para,ihi,2.0);
		else if (ytry >= y[inhi]) {
			/* The rejected point is worse than the second-highest, so look for an intermediate
			lower point, i.e., do a one-dimensional contraction. */
			ysave=y[ihi];
			//ytry=amotry(p,y,psum,ndim,funk,para,ihi,0.5);
			ytry=amotry(p,y,psum,ndim,para,ihi,0.5);
			if (ytry >= ysave) { 	   /* Can't seem to get rid of that high point. Better */
				for (i=1;i<=mpts;i++) {	   /* contract around the lowest (best) point.  */
					if (i != ilo) {
	   					for (j=1;j<=ndim;j++)	p[i][j]=psum[j]=0.5*(p[i][j]+p[ilo][j]);
	   					//y[i]=(*funk)(psum,para);
						y[i]=cost_funct(psum,para);
					}
				}
				*nfunk += ndim; /* Keep track of function evaluations. */
				GET_PSUM /* Recompute psum. */
			}
		} 
		else --(*nfunk); /* Correct the evaluation count. */
	} /* Go back for the test of doneness and the next iteration. */ 
	free_vector(psum,1,ndim);
	return ilo;
}
/************************************************************************/
//double MPC_Base::amotry(double **p, double y[], double psum[], int ndim,
//		double (*funk)(double [], double []), double para[], int ihi, double fac)
double MPC_Base::amotry(double **p, double y[], double psum[], int ndim,
		 double para[], int ihi, double fac)
/* Extrapolates by a factor fac through the face of the simplex across from the 
high point, tries it, and replaces the high point if the new point is better.*/
{
int j;
double fac1,fac2,ytry,*ptry;
      ptry=vector(1,ndim);
      fac1=(1.0-fac)/ndim;
      fac2=fac1-fac;
      for (j=1;j<=ndim;j++) ptry[j]=psum[j]*fac1-p[ihi][j]*fac2;
      ytry=cost_funct(ptry,para); /* Evaluate the function at the trial point. */
      if (ytry < y[ihi]) { /* If it's better than the highest, then replace the highest. */
	y[ihi]=ytry;
	for (j=1;j<=ndim;j++) {
		psum[j] += ptry[j]-p[ihi][j];
		p[ihi][j]=ptry[j];
	}
      }
      free_vector(ptry,1,ndim);
      return ytry;
}
/************************************************************************/
//double MPC_Base::fminsearch(double p0[], int ndim, double ftol,
//				double (*funk)(double [], double []), 
//				double para[], int *nfunk, double uplim[], double lolim[])
double MPC_Base::fminsearch(double p0[], int ndim, double ftol,
				double para[], int *nfunk, double uplim[], double lolim[])
{
	double **p;
	double *y,fmin;
	int i,j,ilo,mpts=ndim+1;

	p=matrix(1,mpts,1,ndim);
	y=vector(1,mpts);
	
	for (j=1;j<=ndim;j++)
	{
		if(p0[j] > uplim[j] || p0[j] < lolim[j])
		{
			cerr << "\nERROR@MPC_Base::fminsearch():j = " << j << "p0 = " << p0[j] << " ,uplim = " << uplim[j] << " ,lolim = " << lolim[j] << "\n";
			system("pause");
			exit(0);
		}
	}

	

	/* Following improvement suggested by L.Pfeffer at Stanford */
	for (i=1;i<=mpts;i++) 
		for (j=1;j<=ndim;j++) 
			p[i][j]=p0[j];
	for (i=1;i<=ndim;i++)
		if (p0[i]==0)
			p[i+1][i]=0.00025;
		else
		{
			if((p0[i]-lolim[i])<(uplim[i]-p0[i]))
				p[i+1][i] = (p0[i]+lolim[i])/2;
			else
				p[i+1][i] = (p0[i]+uplim[i])/2;

			// p[i+1][i]=1.05*p0[i]; // Changed back 2009.07.06
			//p[i+1][i]=(DEP_HIGH+DEP_LOW)/2>p0[i]?(p0[i]+5.0):(p0[i]-5.0);
		}
	//for (i=1;i<=mpts;i++) y[i]=(*funk)(p[i],para);
	for (i=1;i<=mpts;i++) y[i]=cost_funct(p[i],para);

	//ilo=amoeba_constr(p,y,ndim,ftol,funk,para,nfunk,uplim,lolim);
	ilo=amoeba_constr(p,y,ndim,ftol,para,nfunk,uplim,lolim);
	
	if (ilo==0)  // Keep the original point if not converging
//		fmin=(*funk)(p0,para);
		fmin=cost_funct(p0,para);
	// 2008.08.20: new algorithm, no matter which component of ilo exceeds the boundary
	//				just save it and judge the component later
	//else if (ilo==-1)
	//{
	//	for (j=1;j<=ndim;j++) p0[j]=lolim[j];
	//	fmin=(*funk)(p0,para);
	//}
	//else if (ilo==mpts+1)
	//{
	//	for (j=1;j<=ndim;j++) p0[j]=uplim[j];
	//	fmin=(*funk)(p0,para);
	//}
	else
	{
		for (j=1;j<=ndim;j++) p0[j]=p[ilo][j];
		fmin=y[ilo];
	}

	free_vector(y,1,mpts);
	free_matrix(p,1,mpts,1,ndim);

	return fmin;
}
